//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <bsp.h>
#include <flash.h>

int FlashEraseSectors(unsigned long ulFlashBase, long lSectorOffset, long EraseSize);
void FlashWaitForProgram(unsigned long ulFlashBase);
EXTERN_C DECL_CODEINIT int ProgramFlash(unsigned long FlashBase, long Offset, unsigned char *Buffer, long BufferSize);
int FlashWriteBuffer32Mod(unsigned long ulFlashBase,long lOffset,unsigned char *pucData,long lNumBytes);

/*
 *                                                                            *
 * FlashWaitForProgram waits until bit 7 becomes one, indicating that the     *
 * embedded program (be it a chip erase, sector erase, or data store program) *
 * has completed executing.                                                   *
 *                                                                            *
 */
void FlashWaitForProgram(unsigned long ulFlashBase)
{
    unsigned short * volatile pulPtr = (unsigned short *)ulFlashBase;
    unsigned short volatile ulTemp;
    //kprintf("FlashWaitForProgram pulPtr=%08x\n", pulPtr);
    /* Read from the flash ROM until bit 7 is one. */
    while (1) {

        /* Read the status register from the flash ROM. */

        ulTemp = *pulPtr;

        /* Mask off all bits except for bit 7. */

        ulTemp &= 0x0080;

        /* If bit 7 is one, then quit waiting. */

        if (ulTemp == 0x0080) {
            break;
        }
    }
}

EXTERN_C DECL_CODEINIT int ProgramFlash(unsigned long FlashBase, long Offset, unsigned char *Buffer, long BufferSize)
{
    FlashEraseSectors(FlashBase, Offset, BufferSize);
    FlashWriteBuffer32Mod(FlashBase, Offset,Buffer, BufferSize);
    return 0;
}

int
FlashWriteBuffer32Mod(unsigned long ulFlashBase, long lOffset,
                unsigned char *pucData, long lNumBytes)
{
    unsigned short * volatile pulPtr = (unsigned short *) ulFlashBase;
    int volatile i;
    long lIdx;
    long BIdx;
    long ofs_save;

    ofs_save = lOffset;

    for(i=0;i< 128 * 5;i++){}    //Delay 128 us

    // Write the write to buffer command.
    for (lIdx = 0; lIdx < lNumBytes; lIdx += 32) {
        lOffset = ((lIdx + ofs_save) >> 17) <<17;
        // Write buffer data.
        pulPtr[lOffset >> 1] = 0xE8;

        // read status register bit 7 is one.
        FlashWaitForProgram(ulFlashBase + lOffset);
        // write countlegth and block address.
        pulPtr[lOffset >> 1] = 0xf;

        for (BIdx = 0; BIdx < 32; BIdx += 2) {
            pulPtr[(BIdx + lIdx + ofs_save) >> 1] =
                            *((unsigned short *)(pucData + BIdx + lIdx));
        }
        // program buffer to flash confirm.
        pulPtr[lOffset >> 1] = 0xD0;

        // read status register bit 7 is one.
        FlashWaitForProgram(ulFlashBase + lOffset);
    }
       // Wait until the data word has been programmed.
    FlashWaitForProgram(ulFlashBase + lOffset);

    // Put the flash into read array mode.
    pulPtr[0] = 0x00FF;

    return 0;
}

/*
 *
 * FlashEraseSector erases specific number sectors of the flash ROM.
 *
 */
int
FlashEraseSectors(unsigned long ulFlashBase, long lSectorOffset, long EraseSize)
{
    unsigned short * volatile pulPtr = (unsigned short *)ulFlashBase;
    EraseSize += lSectorOffset;

    for (;lSectorOffset < EraseSize; lSectorOffset += 0x20000) {
       //debug('e');
        /* Write the erase command to the flash ROM. */
        pulPtr[lSectorOffset >> 1] = 0x0020;
        pulPtr[lSectorOffset >> 1] = 0x00D0;

        /* Wait until the sector erase has completed. */
        FlashWaitForProgram(ulFlashBase + lSectorOffset);

        /* Put the flash into read array mode. */
        *pulPtr = 0x00FF;
    }
    return 0;
}
